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Rigid Body Motion Estimation based on the Lagrange-d’Alembert 

Principle 


Maziar Izadi^, Amit K. Sanyal^’^ Ernest Barany^ and Sasi P. Viswanathan^ 


Abstract —Stable estimation of rigid body pose and veloci¬ 
ties from noisy measurements, without any knowledge of the 
dynamics model, is treated using the Lagrange-d’Alembert 
principle from variational mechanics. With body-fixed optical 
and inertial sensor measurements, a Lagrangian is obtained 
as the difference between a kinetic energy-like term that is 
quadratic in velocity estimation error and the sum of two 
artificial potential functions; one obtained from a generalization 
of Wahba’s function for attitude estimation and another which 
is quadratic in the position estimate error. An additional 
dissipation term that is linear in the velocity estimation error 
is introduced, and the Lagrange-d’Alembert principle is ap¬ 
plied to the Lagrangian with this dissipation. This estimation 
scheme is discretized using discrete variational mechanics. The 
presented pose estimator requires optical measurements of at 
least three inertially fixed landmarks or beacons in order to 
estimate instantaneous pose. The discrete estimation scheme 
can also estimate velocities from such optical measurements. 
In the presence of bounded measurement noise in the vector 
measurements, numerical simulations show that the estimated 
states converge to a bounded neighborhood of the actual states. 

1. INTRODUCTION 

Estimation of rigid body translational and rotational mo¬ 
tion is indispensable for operations of spacecraft, unmanned 
aerial and underwater vehicles. Autonomous state estimation 
of a rigid body based on inertial vector measurement and 
visual feedback from stationary landmarks, in the absence 
of a dynamics model for the rigid body, is analyzed here. 
This estimation scheme can enhance the autonomy and 
reliability of unmanned vehicles in uncertain GPS-denied 
environments. Salient features of this estimation scheme are: 
(1) use of onboard optical and inertial sensors, with or 
without rate gyros, for autonomous navigation; (2) robustness 
to uncertainties and lack of knowledge of dynamics; (3) 
low computational complexity for easy implementation with 
onboard processors; (4) proven stability with large domain 
of attraction for state estimation errors; and (5) versatile 
enough to estimate motion with respect to stationary as well 
as moving objects. Robust state estimation of rigid bodies 
in the absence of complete knowledge of their dynamics, is 
required for their safe, reliable, and autonomous operations in 
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poorly known conditions. In practice, the dynamics of a vehi¬ 
cle may not be perfectly known, especially when the vehicle 
is under the action of poorly known forces and moments. 
The scheme proposed here has a single, stable algorithm 
for the coupled translational and rotational motion of rigid 
bodies using onboard optical (which may include infra-red) 
and inertial sensors. This avoids the need for measurements 
from external sources, like GPS, which may not be available 
in indoor, underwater or cluttered environments [11]. 

Attitude estimators using unit quaternions for attitude 
representation may be unstable in the sense of Lyapunov, 
unless they identify antipodal quaternions with a single 
attitude. This is also the case for attitude control schemes 
based on continuous feedback of unit quaternions, as shown 
in [1]. One adverse consequence of these unstable estimation 
and control schemes is that they end up taking longer 
to converge compared with stable schemes under similar 
initial conditions and initial transient behavior. Continuous¬ 
time attitude observers and filtering schemes on SO(3) and 
SE(3) have been reported in, e.g., [2], [10], [12], [16], [19]. 
These estimators do not suffer from kinematic singularities 
like estimators using coordinate descriptions of attitude, 
and they do not suffer from unwinding as they do not 
use unit quaternions. The maximum-likelihood (minimum 
energy) filtering method of Mortensen [15] was recently 
applied to attitude estimation, resulting in a nonlinear attitude 
estimation scheme that seeks to minimize the stored “energy” 
in measurement errors [21]. This scheme is obtained by 
applying Hamilton-Jacobi-Bellman (HJB) theory to the state 
space of attitude motion. Since the HJB equation can only be 
approximately solved with increasingly unwieldy expressions 
for higher order approximations, the resulting filter is only 
“near optimal” up to second order. Unlike filtering schemes 
that are based on approximate or “near optimal” solutions 
of the HJB equation and do not have provable stability, 
the estimation scheme obtained here is shown to be almost 
globally asymptotically stable. Moreover, unlike hlters based 
on Kalman filtering, the estimator proposed here does not 
presume any knowledge of the statistics of the initial state 
estimate or the sensor noise. Indeed, for vector measurements 
using optical sensors with limited field-of-view, the probabil¬ 
ity distribution of measurement noise needs to have compact 
support, unlike standard Gaussian noise processes that are 
commonly used to describe such noisy measurements. 

The variational attitude estimator recently appeared in [7], 
where it was shown to be almost globally asymptotically 
stable. Some of the advantages of this scheme over some 
commonly used competing schemes are reported in [6]. This 


paper is the variational estimation framework to coupled 
rotational (attitude) and translational motion, as exhibited 
by maneuvering vehicles like UAVs. In such applications, 
designing separate state estimators for the translational and 
rotational motions may not be effective and may lead to poor 
navigation. Moreover, like other vision-inertial navigation 
schemes [18], the estimation scheme proposed here does 
not rely on GPS. However, unlike many other vision-inertial 
estimation schemes, the estimation scheme proposed here 
can be implemented without any direct velocity measure¬ 
ments. Since rate gyros are usually corrupted by high noise 
content and bias [3], [4], [5], such a velocity measurement- 
free scheme can result in fault tolerance in the case of faults 
with rate gyros. Additionally, this estimation scheme can 
be extended to relative pose estimation between vehicles 
from optical measurements, without direct communications 
or measurements of relative velocities. 

2. NAVIGATION USING OPTICAL AND INERTIAL 
SENSORS 

Consider a vehicle in spatial (rotational and translational) 
motion. Onboard estimation of the pose of the vehicle 


Fig. 1. Inertial landmarks on O as obsei'ved from vehicle S with optical 
measurements. 


involves assigning a coordinate frame fixed to the vehicle 
body, and another coordinate frame fixed in the environment 
which takes the role of the inertial frame. Let O denote 
the observed environment and S denote the vehicle. Let S 
denote a coordinate frame fixed to S and 0 be a coordinate 
frame fixed to O, as shown in Eig.[T] Let R G SO(3) denote 
the rotation matrix from frame S to frame 0 and b denote 
the position of origin of S expressed in frame 0. The pose 
(transformation) from body fixed frame S to inertial frame 
0 is then given by 


g = 


R 

0 


€ SE(3). 


( 1 ) 


Consider vectors known in inertial frame 0 measured by 
inertial sensors in the vehicle-fixed frame S; let /3 be the 
number of such vectors. In addition, consider position vectors 
of a few stationary points in the inertial frame 0 measured 
by optical (vision or lidar) sensors in the vehicle-fixed frame 
S. Velocities of the vehicle may be directly measured or can 
be estimated by linear filtering of the optical position vector 
measurements [8]. Assume that these optical measurements 
are available fory points at time t, whose positions are known 
in frame 0 as pj, j G 1(f), where I{t) denotes the index 
set of beacons observed at time t. Note that the observed 
stationary beacons or landmarks may vary over time due to 
the vehicle’s motion. These points generate (f) unique rel¬ 
ative position vectors, which are the vectors connecting any 
two of these landmarks. When two or more position vectors 
are optically measured, the number of vector measurements 
that can be used to estimate attitude is (^) -|- /3. This number 
needs to be at least two (i.e., ( 2 ) + /3 > 2) at an instant, 
for the attitude to be uniquely determined at that instant. In 
other words, if at least two inertial vectors are measured at 
all instants (i.e., /3 > 2), then beacon position measurements 
are not required for estimating attitude. However, at least one 
beacon or feature point position measurement is still required 
to estimate the position of the vehicle. 

A. Pose Measurement Model 

Denote the position of an optical sensor and the unit vector 
from that sensor to an observed beacon in frame S as G 
and G S^, fc = 1,..., ^, respectively. Denote the relative 
position of the stationary beacon observed by the 
sensor expressed in frame S as q^. Thus, in the absence of 
measurement noise 

Pj = R{qj -f s 6 = Rttj -f 6, j G T(f), (2) 

where Uj = are positions of these points expressed in 

S. In practice, the Oj are obtained from range measurements 
that have additive noise, which we denote as aj*. In the case 
of lidar range measurements, these are given by 

< = {q-r + s" = + s\ j G X(f), (3) 

where is the measured range to the point by the 

sensor. The mean of the vectors pj and a™ are denoted as p 
and o'" respectively, and satisfy 

= + ( 4 ) 

J J 

where p = - Pj’ = “ X) ^ ’^^e additive 

'' i=i '' i=i 

measurement noise obtained by averaging the measurement 
noise vectors for each of the Oj. Consider the (^) relative 
position vectors from optical measurements, denoted as dj = 
P\ — Pi in frame 0 and the corresponding vectors in frame 
S as Ij = a\— at, for i. The /3 measured 

inertial vectors are included in the set of dj, and their 
corresponding measured values expressed in frame S are 
included in the set of Ij. If the total number of measured 
(optical and inertial) vectors, ( 2 ) + /3 = 2, then I 3 = h x I 2 






is considered a third measured direction in frame S with 
corresponding vector ds = di x d 2 in frame 0. Therefore, 

dj = Rlj ^D = RL, ( 5 ) 

where D = [di ■ ■ ■ dn], L = [li • • • S with 

n = 3 if (^) + /3 = 2 and n = Q + /3 if (^) + /3 > 2. 
Note that the matrix D consists of vectors known in frame 
0. Denote the measured value of matrix L in the presence 
of measurement noise as L™. Then, 

L™ = r'^D + , (6) 

where G consists of the additive noise in the vector 

measurements made in the body frame S. 


B. Velocities Measurement Model 


Denote the angular and translational velocity of the rigid 
body expressed in frame S by and v, respectively. Thus, 
one can write the kinematics of the rigid body as 

tl = Rn^,h = Rv^g = gC', (7) 


where ^ = 


and (•)^ 


3 ^ = [f o" 

R^ —so(3) C R^^^ is the skew-symmetric cross-product 
operator that gives the vector space isomorphism between 
R^ and so(3). For the general development of the motion 
estimation scheme, it is assumed that the velocities are di¬ 
rectly measured. The estimator is then extended to cover the 
cases where; (i) only angular velocity is directly measured; 
and (ii) none of the velocities are directly measured. 


3. DYNAMIC ESTIMATION OF MOTION FROM 
PROXIMITY MEASUREMENTS 
In order to obtain state estimation schemes from mea¬ 
surements as outlined in Section |2] in continuous time, 
the Lagrange-d’Alembert principle is applied to an action 
functional of a Lagrangian of the state estimate errors, with 
a dissipation term linear in the velocities estimate error. This 
section presents the estimation scheme obtained using this 
approach. Denote the estimated pose and its kinematics as 

gSE( 3), i = g|^, (8) 



where f is rigid body velocities estimate, with go as the 
initial pose estimate and the pose estimation error as 


h = 


1 _ 

Q 

b-Qb 


Q X 


0 

1 


0 1 


eSE(3), (9) 


where Q = RFt^ is the attitude estimation error and x = b — 
Qb. Then one obtains, in the case of perfect measurements. 


li = h(^^, where ipig.C'A) = 


Adg(r-d, 

( 10 ) 


where Ad„ = 




0 

31 


3L 

0 


The attitude 


for^ = 

and position estimation error dynamics are also in the form 
Q = Qu}^, x = Qv. (11) 


A. Lagrangian from Measurement Residuals 

Consider the sum of rotational and translational measure¬ 
ment residuals between the measurements and estimated pose 
as a potential energy-like function. Defining the trace inner 
product on R"iX ”2 

(Ai, A 2 ) := trace(A7A2), (12) 

the rotational potential function (Wahba’s cost function [20]) 
is expressed as 

W"(g,L™,79) = (13) 

where W = diag(rt;j) G R"^" is a positive diagonal 
matrix of weight factors for the measured /™. Consider the 
translational potential function 

Ut{g,a^,p) = ]^Ky^y = ]^K,\\p-Rd'^ - , (14) 

where p is defined by (|4|i, y = y{g, d™,p) = p—Ra^ — b and 
K is a positive scalar. Therefore, the total potential function 
is defined as the sum of the generalization of (fljl l defined 
in [7], [17] for attitude determination on SO(3), and the 
translational energy (O as 

U{g,L^,D,d^,p) = $(W°(g,L”^,i9)) +Ut{g,d'^,p) 

= {D - RL'^)W)) 

+ lK\\p-RbJ^-b\\\ (15) 

where W is positive definite (not necessarily diagonal) which 
can be selected according to Lemma 3.2 in [7], and $ : 
[0, 00) 1 -^ [0, 00) is a function that satisfies $(0) = 0 and 
$'(;(:) > 0 for all G [0,c»). Eurthermore, $'(•) < a{-) 
where «(■) is a Class-/C function [9] and 4)'(-) denotes the 
derivative of 4>(-) with respect to its argument. Because of 
these properties of the function 4>, the critical points and their 
indices coincide for and [7]. Define the kinetic 

energy-like function: 

(i6) 

where JJ G R®^® > 0 is an artificial inertia-like kernel 
matrix. Note that in contrast to rigid body inertia matrix, J is 
not subject to intrinsic physical constraints like the triangle 
inequality, which dictates that the sum of any two eigenvalues 
of the inertia matrix has to be larger than the third. Instead, 
J is a gain matrix that can be used to tune the estimator. 
Eor notational convenience, (p{g, is denoted as (p from 

now on; this quantity is the velocities estimation error in the 
absence of measurement noise. Now define the Lagrangian 

£(g, L^, D, cr,p, p) = Tip) - U{g, A™, D, d™,p), (17) 


and the corresponding action functional over an arbitrary 
time interval [to,T] for T > 0, 


S{Cig,L^,D,d^,P,p)) = r Cig,L"^,D,d^T,p)dt, 

Jtn 


( 18 ) 


















such that g = g(^)^. A Rayleigh dissipation term linear 
in the velocities of the form Dtp where D e > 0 is 

used in addition to the Lagrangian (fTTI) . and the Lagrange- 
d’Alembert principle from variational mechanics is applied 
to obtain the estimator on TSE(3). This yields 

pT 

Sh^^S[jC{h,D,p,ip)) = / ry’^Dtpdf, (19) 

Jto 

which in turn results in the following continuous-time filter. 


B. Variational Estimator for Pose and Velocities 

The nonlinear variational estimator obtained by applying 
the Lagrange-d’Alembert principle to the Lagrangian (fTTI) 
with a dissipation term linear in the velocities estimation 
error, is given by the following statement. 

Theorem 3.1: The nonlinear variational estimator for pose 
and velocities is given by 

=ad*J(p-Z(g,L™,£),d™,p)-D(p, 

h (20) 

[k =m\ 


where adj = (ad^)^ with ad^ defined by 


adr = 


for C = 


and Z(g, L"*, Z?, a™,p) is defined by 
Z(g,L-,Z9,d'",p) = 


(j) 


'(wO(g,L™,i9))5r(ZZ)+KpXy 


Ky 


( 21 ) 


( 22 ) 


where is defined as (fT3] l. y = y{g,a'^,p) = 

p — RaP^ — S and 


S'r(^) = Yeyi{DW{L^ f - RL'^WD^), (23) 


where vex(-) : so(3) ^ R.^ is the inverse of the (•)^ map. 
The proof is presented in [8], [14]. In the proposed approach, 
the time evolution of (g, has the form of the dynamics of a 
rigid body with Rayleigh dissipation. This results in an esti¬ 
mator for the motion states (g, that dissipates the “energy” 
content in the estimation errors (h, tp) = (gg“^, Adg(^ — f)) 
to provide guaranteed asymptotic stability in the case of 
perfect measurements [7]. 

Explicit expressions for the vector of velocities can 
be obtained for two common cases when these velocities are 
not directly measured. These cases are presented here. 


C. Variational Estimator Implemented without Direct Veloc¬ 
ity Measurements 

The velocity measurements in ( l20b can be replaced by 
filtered velocity estimates obtained by linear filtering of 
optical and inertial measurements using, e.g., a second-order 
Butterworth filter. This is both useful and necessary when 
velocities are not directly measured. 


1) Angular velocity is measured using rate gyros: For 
the case that angular velocities are measured by rate gyros 
besides the j feature point position measurements, the linear 
velocities of the rigid body can be calculated using each 
single position measurement by rewriting (l26l l as 


-u/, (24) 

for the point. Averaging the values of i/ derived from 
all feature points gives a more reliable result. Therefore, the 
rigid body’s filtered velocities are expressed in this case as 


e/ = 






i=i 



(25) 


2) Translational and angular velocity measurements are 
not available: In this case, rigid body velocities can be 
calculated in terms of the measurements. In order to do so, 
one can differentiate (|2]i as follows 


pj = Rfl^Oj Rdj -\-b = R(fl^aj dj u) =0 

=^dj — V = 0 

^Vj = Qj = [aj - /]C = (26) 

where G{aj) = [a^ — I] has full row rank. From vision- 
based or Doppler lidar sensors, one can also measure the 
velocities of the observed points in frame S, denoted u™. 
Here, velocity measurements as would be obtained from 
vision-based sensors is considered. The measurement model 
for the velocity is of the form 


t;™ = G(a,)e + ^„ (27) 

where Pj G R^ is the additive error in velocity measure¬ 
ment vj^. Instantaneous angular and translational velocity 
determination from such measurements is treated in [17]. 
Note that Vj = dj, for j G E{f). As this kinematics 
indicates, the relative velocities of at least three beacons are 
needed to determine the vehicle’s translational and angular 
velocities uniquely at each instant. The rigid body velocities 
are obtained using the pseudo-inverse of G(A^); 

G{Af)^f = Y{V^) ^ = Gi{Af)Y{Vf), (28) 



-G{a{)- 


r fi 

Vi 

where G(A'^) = 


and Y(y^) = 



G{af). 


f 

Vj 


for 1, ...J G E{t). When at least three beacons are mea¬ 
sured, G(A'^) is a full column rank matrix, and = 

gives its pseudo-inverse. 

4. DISCRETIZATION FOR COMPUTER 
IMPLEMENTATION 

For onboard computer implementation, the variational 
estimation scheme outlined above has to be discretized. Since 
the estimation scheme proposed here is obtained from a 
variational principle of mechanics, it can be discretized by 
applying the discrete Lagrange-d’Alembert principle [13]. 














Consider an interval of time [t[),T] S K’*' separated into 
N equal-length subintervals for i = 0,1,...,iV, 

with t]s[ = T and — ti = At is the time step size. Let 
(giiCi) £ SE(3) X K.® denote the discrete state estimate at 
time U, such that (gj,|i) « (g(fi),where (g(t),^(f)) 
is the exact solution of the continuous-time estimator at time 
t S [tQ,T]. Let the values of the discrete-time measurements 
o'" and L"* at time ti be denoted as a™ and L™, 
respectively. Further, denote the corresponding values for the 
latter two quantities in inertial frame at time ti by pi and 
Di, respectively. The discrete-time filter is then presented in 
the form of a Lie group variational integrator (LGVI) in the 


following statement. 

Theorem 4.1: A first-order discretization of the estimator 
proposed in Theorem 13.11 is given by 

=^^{Fa-J fJ), (30) 

(M + Amt)v^+l = fJMv, (31) 

+ Atnibi+i + RiJ^io^i — Pi+i), 

(J -I- AfDj.)wi+i = fJ Joji + AtMvi+i X Vi+i 

+AtKpf^^(bi.^.l + Ri+ia^i) (32) 

-At^' (g,+i , L™ 1 , A+i )) (4+1) , 

I, (33) 

g*+i = g*exp(A4^), (34) 


where Fi G SO(3), (g(fo)4(^o)) = (go4o), J is defined 
in terms of positive matrix J hy J = i trace [J]/ — J, M 
is a positive definite matrix, (pi = [ujJ and Sri{Ri) is 

the value of Sr{R) at time ti, with Sr{R) defined by 
The proof is presented in [8]. 

5. NUMERICAL SIMULATIONS 

This section presents numerical simulation results for the 
discrete-time estimator obtained in Section H] In order to 
numerically simulate this estimator, simulated true states of 
an aerial vehicle flying in a cubical room are produced using 
the kinematics and dynamics equations of a rigid body. The 
vehicle mass and moment of inertia are taken to be = 420 
g and Jy = [51.2 60.2 59.6]"*' g.m^, respectively. The 

resultant external forces and torques applied on the vehicle 
are = 10“^[10cos(0.1f) 2sin(0.2f) — 2sin(0.5f)]'*' 
N and r„(f) = 10~^(j)y{t) N.m, respectively. The room is 
assumed to be a cube of size lOmxlOmxlOm with the 
inertial frame origin at the geometric center. The initial 
attitude and position of the vehicle are: 

Ro = expmsoo) ((J x ^ 7]^) ’ 

and bo = [2.5 0.5 - 3]'*’ m. (35) 

This vehicle’s initial angular and translational velocity re¬ 
spectively, are: 

Uo = [0.2 - 0.05 O.!]"*" rad/s, 
and I/O = [-0.05 0.15 0.03]'*' m/s. 



Fig. 2. Position and attitude trajectory of the simulated vehicle. 


The vehicle dynamics is simulated over a time interval of 
T = 150 s, with a time stepsize of At = 0.02 s. The 
trajectory of the vehicle over this time interval is depicted in 
Fig. 12] The following two inertial directions, corresponding 
to nadir and Earth’s magnetic field direction, are measured 
by the inertial sensors on the vehicle: 

di = [0 0 - 1]*', d2 = [0.1 0.975 - 0.2]*’. (37) 

For optical measurements, eight beacons are located at the 
eight vertices of the cube, labeled 1 to 8. The positions of 
these beacons are known in the inertial frame and their index 
(label) and relative positions are measured by optical sensors 
onboard the vehicle whenever the beacons come into the 
field of view of the sensors. Three identical cameras (optical 
sensors) and inertial sensors are assumed to be installed 
on the vehicle. The cameras are fixed to known positions 
on the vehicle, on a hypothetical horizontal plane passing 
through the vehicle, 120° apart from each other, as shown in 
Fig.di All the camera readings contain random zero mean 
signals whose probability distributions are normalized bump 
functions with width of 0.001m. The following are selected 
for the positive definite estimator gain matrices: 

J = diag([0.9 0.6 0.3]), 

M = diag([0.0608 0.0486 0.0365]), (38) 

Dr = diag([2.7 2.2 1.5]),Dt = diag([0.1 0.12 0.14]). 

<!)(•) could be any function with the properties described 
in Section|3] but is selected to be <i)(x) = x here. The initial 
state estimates have the following values: 

go = /, ho = [0.1 0.45 0.05]* rad/s, 

and uo = [2.05 0.64 1.29]* m/s. 

A conic field of view (FOV) of 2x40° is assumed for the 
cameras, which guarantees at least three beacons observed 
are common between successive measurements. The vehi¬ 
cle’s velocity vector is calculated from (l28T l. The discrete¬ 
time estimator (l30l) - (l34l) is simulated over a time of T = 20 









t (s) 


Fig. 3. Principal angle of the attitude and position estimation error for 
CASE 1. 




t (s) 


Fig. 4. Angular and translational velocity estimation error for CASE 1. 


s with time stepsize At = 0.02 s. At each measurement 
instant, ( l30l l is solved using Newton-Raphson iterations to 
find an approximation for F^. The remaining equations (all 
explicit) are solved consecutively to generate the estimated 
states. The principal angle of the attitude estimation error 
and the position estimate error are plotted in Fig. [3 The an¬ 
gular and translational components of the vehicle’s velocity 
estimate errors are also depicted in Fig. |4] 

6. CONCLUSION 

This article proposes an estimator for rigid body pose and 
velocities, using optical and inertial measurements by sensors 
onboard the rigid body. The sensors are assumed to provide 
measurements in continuous-time or at a sufficiently high 
frequency, with bounded noise. An artificial kinetic energy 
quadratic in rigid body velocity estimate errors is defined, 
as well as two fictitious potential energies: (1) a generalized 
Wahba’s cost function for attitude estimation error in the 
form of a Morse function, and (2) a quadratic function of 
the vehicle’s position estimate error. Applying the Lagrange- 


d’Alembert principle on a Lagrangian consisting of these 
energy-like terms and a dissipation term linear in velocities 
estimation error, an estimator is designed on the Lie group 
of rigid body motions. A discrete-time counterpart of this 
estimator is also presented. In the presence of measurement 
noise, numerical simulations show that state estimates con¬ 
verge to a bounded neighborhood of the true states. 
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